#include "PID.hpp"

PIDControl::~PIDControl()
{
}

PIDControl::PIDControl(float kp, float ki, float kd, float integerrlimit, float outlimit)
{
    this->Kp = kp;
    this->Ki = ki;
    this->Kd = kd;
    this->Integerrlimit = integerrlimit;
    this->Outputlimit = outlimit;
    Last_D = 0;
    Integral = 0;
    Output = 0;
    LastOutput = 0;
    LastError = 0;
}

void PIDControl::PID_Config(float kp, float ki, float kd)
{
    this->Kp = kp;
    this->Ki = Ki;
    this->Kd = Kd;
}
/**
 * @brief PID运算 输入设置值和测量值的差值
 *
 * @param error 误差值
 * @return float 返回输出值
 */
float PIDControl::operator()(float error)
{
    float P = Kp * error;
    Integral += Ki * P;
    if (Integral > Integerrlimit)
        Integral = Integerrlimit;
    if (Integral < -Integerrlimit)
        Integral = -Integerrlimit;
    float D = Kd * (error - LastError);
    Last_D += K * (D - Last_D);
    D = Last_D;
    Output = P + Integral + D;
    if (Output > Outputlimit)
        Output = Outputlimit;
    if (Output < -Outputlimit)
        Output = -Outputlimit;
    LastOutput = Output;
    LastError = error;
    return Output;
}